#include "LiftStretch.h"

auto pos_pid = MM::PIDInstance(6.0f, 0.0f, 0.0f, 2000.0f, 1000.0f, 10.0f);
auto pos_array = std::array<MM::PIDInstance, 8>{pos_pid, pos_pid};

auto vel_pid = MM::PIDInstance(5.0f, 0.3f, 0.0f, 15000.0f, 1000.0f, 10.0f);
auto vel_array = std::array<MM::PIDInstance, 8>{vel_pid, vel_pid};


LiftStretch &LiftStretch::getInstance() {
    static LiftStretch _instance;
    return _instance;
}

void LiftStretch::posVelControl(const float liftPos, const float liftVel, const float stretchPos,
                                const float stretchVel) {
    m3508s.posVelControl({nullopt, nullopt, -liftPos, +liftPos,},
                         {0, 0, -liftVel, +liftVel,}, pos_array, vel_array);
}

LiftStretch::LiftStretch(): m3508s("can1", "lift&stretch", true, false) {
    m3508s.enable();
}
